#!/usr/bin/env python3

import json
import time
import os
import requests
import subprocess
import struct
import threading
from vosk import Model

class RoboticArmVoiceControl:
    def __init__(self):
        # 尝试多个可能的模型路径
        possible_paths = [
            "model",
            "vosk-model-small-cn-0.22",
            "/root/voice-interaction-robotic-arm/model",
            "/root/voice-interaction-robotic-arm/vosk-model-small-cn-0.22"
        ]
        
        model_path = None
        for path in possible_paths:
            if os.path.exists(path):
                model_path = path
                break
        
        if model_path is None:
            print("❌ 未找到语音模型文件")
            print("请执行以下命令下载模型：")
            print("cd ~/voice-interaction-robotic-arm")
            print("wget https://alphacephei.com/vosk/models/vosk-model-small-cn-0.22.zip")
            print("unzip vosk-model-small-cn-0.22.zip")
            exit(1)
            
        print(f"✅ 使用模型路径: {model_path}")
        self.model = Model(model_path)
        
        self.api_key = "ms-fecb8eca-0503-44e9-9cf2-4870bb0ed37c"
        self.base_url = "https://api-inference.modelscope.cn/v1"
        self.model_name = "deepseek-ai/DeepSeek-V3.2-Exp"
        
        # 检查系统TTS工具
        self.tts_tool = self.check_tts_tools()
        print(f"🔊 使用的TTS工具: {self.tts_tool}")
        
        # 检测音频设备
        self.audio_device = self.detect_audio_device()
        print(f"🎵 使用的音频设备: {self.audio_device}")
        
        # 编译C程序
        self.compile_c_programs()
        
        # 机械臂状态
        self.arm_state = {
            "base": 90,
            "shoulder": 90,
            "elbow": 90,
            "wrist": 90,
            "gripper": 0
        }
        
        print("🤖 机械臂语音控制系统已启动")
        print("📍 当前舵机状态:", self.arm_state)
    
    def detect_audio_device(self):
        """检测可用的音频设备"""
        try:
            # 使用 arecord -l 和 aplay -l 检测设备
            result = subprocess.run(['arecord', '-l'], capture_output=True, text=True)
            if result.returncode == 0:
                lines = result.stdout.split('\n')
                for line in lines:
                    if 'card' in line and 'device' in line:
                        # 查找 USB 设备
                        if 'USB' in line or 'BY' in line:
                            # 提取卡号和设备号
                            parts = line.split(':')
                            if len(parts) > 0:
                                card_part = parts[0]
                                card_num = card_part.split('card')[-1].strip()
                                return f"hw:{card_num},0"
            
            # 如果没有找到 USB 设备，使用默认设备
            return "default"
        except Exception as e:
            print(f"⚠️  音频设备检测失败: {e}")
            return "default"
    
    def compile_c_programs(self):
        """编译C语言音频程序"""
        print("🔨 编译音频程序...")
        
        # 编译录音程序 - 使用固定的设备名称
        if not os.path.exists("record"):
            # 创建使用正确设备的record.c版本
            self.create_fixed_record_c()
            result = subprocess.run([
                "gcc", "-o", "record", "record_fixed.c", 
                "-lasound", "-lopus", "-lm"
            ], capture_output=True, text=True)
            if result.returncode != 0:
                print(f"❌ 编译record失败: {result.stderr}")
                return False
        
        # 编译播放程序 - 使用固定的设备名称
        if not os.path.exists("play"):
            # 创建使用正确设备的play.c版本
            self.create_fixed_play_c()
            result = subprocess.run([
                "gcc", "-o", "play", "play_fixed.c",
                "-lasound", "-lopus", "-lm"
            ], capture_output=True, text=True)
            if result.returncode != 0:
                print(f"❌ 编译play失败: {result.stderr}")
                return False
        
        print("✅ 音频程序编译完成")
        return True
    
    def create_fixed_record_c(self):
        """创建固定设备配置的record.c"""
        record_content = '''
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <time.h>
#include <alsa/asoundlib.h>
#include <opus/opus.h>

#define DEFAULT_DURATION 6
#define MAX_DURATION 30

struct recorder {
    snd_pcm_t* device;
    int channels;
    int sample_rate;
    int frames_per_buffer;  // 320 @ 16kHz, 20ms
    OpusEncoder* encoder;
    opus_int16* buffer;
};

int recorder_create(struct recorder* rec, int sample_rate, int channels) {
    int err;
    snd_pcm_hw_params_t* params;

    rec->sample_rate = sample_rate;
    rec->channels = channels;
    rec->frames_per_buffer = (sample_rate * 20) / 1000; // 固定20ms

    // 使用默认设备
    if ((err = snd_pcm_open(&rec->device, "default", SND_PCM_STREAM_CAPTURE, 0)) < 0) {
        fprintf(stderr, "无法打开录音设备: %s\\n", snd_strerror(err));
        return -1;
    }

    snd_pcm_hw_params_alloca(&params);
    snd_pcm_hw_params_any(rec->device, params);
    snd_pcm_hw_params_set_access(rec->device, params, SND_PCM_ACCESS_RW_INTERLEAVED);
    snd_pcm_hw_params_set_format(rec->device, params, SND_PCM_FORMAT_S16_LE);
    snd_pcm_hw_params_set_channels(rec->device, params, channels);
    unsigned int rate = sample_rate;
    snd_pcm_hw_params_set_rate_near(rec->device, params, &rate, 0);

    if ((err = snd_pcm_hw_params(rec->device, params)) < 0) {
        fprintf(stderr, "无法设置录音参数: %s\\n", snd_strerror(err));
        snd_pcm_close(rec->device);
        return -1;
    }

    if ((err = snd_pcm_prepare(rec->device)) < 0) {
        fprintf(stderr, "无法准备录音设备: %s\\n", snd_strerror(err));
        snd_pcm_close(rec->device);
        return -1;
    }

    int error;
    rec->encoder = opus_encoder_create(sample_rate, channels, OPUS_APPLICATION_VOIP, &error);
    if (!rec->encoder || error != OPUS_OK) {
        fprintf(stderr, "Opus 编码器创建失败: %s\\n", opus_strerror(error));
        snd_pcm_close(rec->device);
        return -1;
    }

    // 推荐设置比特率（语音足够）
    opus_encoder_ctl(rec->encoder, OPUS_SET_BITRATE(24000));

    rec->buffer = malloc(sizeof(opus_int16) * rec->frames_per_buffer * channels);
    if (!rec->buffer) {
        fprintf(stderr, "内存分配失败\\n");
        opus_encoder_destroy(rec->encoder);
        snd_pcm_close(rec->device);
        return -1;
    }

    return 0;
}

void recorder_destroy(struct recorder* rec) {
    if (rec->buffer) free(rec->buffer);
    if (rec->encoder) opus_encoder_destroy(rec->encoder);
    if (rec->device) snd_pcm_close(rec->device);
    memset(rec, 0, sizeof(*rec));
}

int record_audio(struct recorder* rec, FILE* fp) {
    int frames_read = snd_pcm_readi(rec->device, rec->buffer, rec->frames_per_buffer);
    if (frames_read < 0) {
        // 尝试恢复
        fprintf(stderr, "读取音频数据失败: %s\\n", snd_strerror(frames_read));
        snd_pcm_recover(rec->device, frames_read, 0);
        return -1;
    }
    
    if (frames_read != rec->frames_per_buffer) {
        fprintf(stderr, "读取帧数不匹配: %d != %d\\n", frames_read, rec->frames_per_buffer);
        return -1;
    }

    unsigned char encoded_data[1276];
    int len = opus_encode(rec->encoder, rec->buffer, rec->frames_per_buffer, encoded_data, sizeof(encoded_data));
    if (len <= 0) {
        // 静音或编码失败，跳过
        return 0;
    }

    int32_t packet_len = len;
    if (fwrite(&packet_len, sizeof(packet_len), 1, fp) != 1 ||
        fwrite(encoded_data, 1, len, fp) != (size_t)len) {
        perror("写入文件失败");
        return -1;
    }

    return len;
}

// 获取当前毫秒时间（用于精确计时）
long long get_time_ms() {
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    return ts.tv_sec * 1000LL + ts.tv_nsec / 1000000LL;
}

int main(int argc, char* argv[]) {
    int duration = DEFAULT_DURATION;
    if (argc > 1) {
        duration = atoi(argv[1]);
        if (duration <= 0 || duration > MAX_DURATION) {
            fprintf(stderr, "警告: 时长应为 1~%d 秒，使用默认 %d 秒。\\n", MAX_DURATION, DEFAULT_DURATION);
            duration = DEFAULT_DURATION;
        }
    }

    struct recorder rec = {0};
    if (recorder_create(&rec, 16000, 1) != 0) {
        return EXIT_FAILURE;
    }

    FILE* fp = fopen("input.opus", "wb");
    if (!fp) {
        perror("无法创建 input.opus");
        recorder_destroy(&rec);
        return EXIT_FAILURE;
    }

    printf("[INFO] 开始录音 %d 秒...\\n", duration);

    long long start_time = get_time_ms();
    long long end_time = start_time + duration * 1000;
    int frame_count = 0;

    while (get_time_ms() < end_time) {
        if (record_audio(&rec, fp) < 0) {
            break;
        }
        frame_count++;
        // 每帧约20ms，无需额外 sleep
    }

    fclose(fp);
    recorder_destroy(&rec);

    printf("[INFO] 录音结束，共 %d 帧（%.1f 秒）\\n", frame_count, frame_count * 0.02);
    return EXIT_SUCCESS;
}
'''
        with open("record_fixed.c", "w") as f:
            f.write(record_content)
    
    def create_fixed_play_c(self):
        """创建固定设备配置的play.c"""
        play_content = '''
/*
 * Opus音频播放器（修正版）
 * 播放由 record.c 生成的带 4 字节帧头的自定义 Opus 文件
 */

#include <stdio.h>
#include <stdlib.h>
#include <alsa/asoundlib.h>   // ALSA 音频库
#include <opus/opus.h>        // Opus 编解码库
#include <stdint.h>           // uint32_t
#include <inttypes.h>         // PRIu32

#define IFILE "record.opus"   // 输入文件名

// 播放器结构体
struct player {
    snd_pcm_t* device;
    int channels;
    int sample_rate;
    int duration;             // 周期时长（毫秒）
    int frames;               // 每周期帧数
    OpusDecoder* decoder;
    opus_int16* buffer;
};

// 创建播放器
struct player* player_create(const char* name, int sample_rate, int channels, int duration)
{
    struct player* player = malloc(sizeof(struct player));
    if (!player) return NULL;

    player->channels = channels;
    player->sample_rate = sample_rate;
    player->duration = duration;

    // 使用默认设备
    int error = snd_pcm_open(&player->device, "default", SND_PCM_STREAM_PLAYBACK, 0);
    if (error < 0) {
        fprintf(stderr, "打开设备 %s 失败: %s\\n", name, snd_strerror(error));
        free(player);
        return NULL;
    }

    error = snd_pcm_set_params(player->device,
                               SND_PCM_FORMAT_S16_LE,
                               SND_PCM_ACCESS_RW_INTERLEAVED,
                               player->channels,
                               player->sample_rate,
                               1,
                               player->duration * 1000);
    if (error < 0) {
        fprintf(stderr, "设置PCM参数失败: %s\\n", snd_strerror(error));
        snd_pcm_close(player->device);
        free(player);
        return NULL;
    }

    player->decoder = opus_decoder_create(player->sample_rate, player->channels, &error);
    if (error != OPUS_OK) {
        fprintf(stderr, "创建Opus解码器失败: %s\\n", opus_strerror(error));
        snd_pcm_close(player->device);
        free(player);
        return NULL;
    }

    player->frames = player->sample_rate * player->duration / 1000;
    player->buffer = malloc(sizeof(opus_int16) * player->frames * player->channels);
    if (!player->buffer) {
        fprintf(stderr, "内存分配失败\\n");
        opus_decoder_destroy(player->decoder);
        snd_pcm_close(player->device);
        free(player);
        return NULL;
    }

    return player;
}

// 销毁播放器
void player_destroy(struct player *player)
{
    if (!player) return;
    if (player->buffer) free(player->buffer);
    if (player->device) snd_pcm_close(player->device);
    if (player->decoder) opus_decoder_destroy(player->decoder);
    free(player);
}

// 启动播放
int player_start(struct player *player)
{
    return player ? snd_pcm_start(player->device) : -1;
}

// 停止播放
int player_stop(struct player *player)
{
    return player ? snd_pcm_drain(player->device) : -1;
}

// 播放一帧数据
int play_audio(struct player* player, char* data, size_t size)
{
    if (!player) return -1;

    int dec_frames = opus_decode(player->decoder, (unsigned char*)data, (opus_int32)size, player->buffer, player->frames, 0);
    if (dec_frames < 0) {
        fprintf(stderr, "Opus解码失败: %s\\n", opus_strerror(dec_frames));
        return -1;
    }

    snd_pcm_sframes_t frames = snd_pcm_writei(player->device, player->buffer, dec_frames);
    if (frames < 0) {
        fprintf(stderr, "PCM写入失败: %s\\n", snd_strerror(frames));
        return -1;
    }

    return 0;
}

// 全局文件缓冲区
char file_buffer[4096];

int main()
{
    FILE *fp = fopen(IFILE, "rb");
    if (!fp) {
        perror(IFILE);
        return EXIT_FAILURE;
    }

    struct player* player = player_create("default", 16000, 1, 20);
    if (!player) {
        fclose(fp);
        return EXIT_FAILURE;
    }

    player_start(player);

    while (1) {
        uint32_t rbytes;  // 帧数据长度（无符号）

        // 尝试读取4字节帧头
        if (fread(&rbytes, 1, sizeof(rbytes), fp) != sizeof(rbytes)) {
            if (feof(fp)) {
                puts("播放完成");
            } else {
                perror("读取帧头失败");
            }
            break;
        }

        // 检查是否超出缓冲区
        if (rbytes > sizeof(file_buffer)) {
            fprintf(stderr, "帧大小 %" PRIu32 " 超过缓冲区容量 %zu\\n", rbytes, sizeof(file_buffer));
            break;
        }

        // 读取实际帧数据
        size_t bytes = fread(file_buffer, 1, rbytes, fp);
        if (bytes != rbytes) {
            fprintf(stderr, "期望读取 %" PRIu32 " 字节，实际读取 %zu 字节\\n", rbytes, bytes);
            break;
        }

        printf("读取 %zu 字节\\n", bytes);

        if (play_audio(player, file_buffer, bytes) < 0) {
            // 可选择继续或退出
            continue;
        }
    }

    player_stop(player);
    player_destroy(player);
    fclose(fp);

    return EXIT_SUCCESS;
}
'''
        with open("play_fixed.c", "w") as f:
            f.write(play_content)
    
    def check_tts_tools(self):
        """检查可用的TTS工具"""
        tools = ['espeak', 'espeak-ng']
        
        for tool_name in tools:
            try:
                result = subprocess.run(['which', tool_name], 
                                      capture_output=True, text=True)
                if result.returncode == 0:
                    print(f"✅ 找到TTS工具: {tool_name}")
                    return tool_name
            except Exception as e:
                print(f"❌ {tool_name} 检查失败: {e}")
                continue
        
        print("⚠️  未找到可用的TTS工具，将使用文本提示")
        return None
    
    def text_to_speech(self, text):
        """文本转语音播报"""
        if not self.tts_tool:
            print(f"🔊 [无语音] {text}")
            self.play_beep()
            return
        
        try:
            print(f"🔊 [语音播报] {text}")
            
            if self.tts_tool in ['espeak', 'espeak-ng']:
                cmd = [self.tts_tool, '-v', 'zh', '-s', '150', text]
                subprocess.Popen(cmd, 
                               stdout=subprocess.DEVNULL, 
                               stderr=subprocess.DEVNULL)
                
        except Exception as e:
            print(f"❌ 语音播报失败: {e}")
            self.play_beep()
    
    def speak_and_wait(self, text, wait_time=1):
        """语音播报并等待"""
        print(f"🔊 {text}")
        self.text_to_speech(text)
        time.sleep(wait_time)
    
    def play_beep(self):
        """播放蜂鸣声"""
        try:
            os.system('echo -e "\a"')
            time.sleep(0.1)
        except:
            pass

    def record_audio_file(self, duration=3, filename="temp_record.opus"):
        """使用C程序录制音频"""
        try:
            print(f"🎤 开始录音 {duration} 秒...")
            
            # 使用编译好的record程序录音
            result = subprocess.run(
                ["./record", str(duration)],
                capture_output=True,
                text=True,
                timeout=duration + 2
            )
            
            if result.returncode != 0:
                print(f"❌ 录音失败: {result.stderr}")
                return False
            
            # 重命名文件
            if os.path.exists("input.opus"):
                if os.path.exists(filename):
                    os.remove(filename)
                os.rename("input.opus", filename)
                print(f"✅ 录音完成: {filename}")
                return True
            else:
                print("❌ 录音文件未生成")
                return False
                
        except subprocess.TimeoutExpired:
            print("❌ 录音超时")
            return False
        except Exception as e:
            print(f"❌ 录音异常: {e}")
            return False

    def play_audio_file(self, filename="temp_record.opus"):
        """使用C程序播放音频"""
        try:
            if not os.path.exists(filename):
                print(f"❌ 音频文件不存在: {filename}")
                return False
            
            print("🔊 播放音频...")
            
            # 重命名为play程序期望的文件名
            if filename != "record.opus":
                if os.path.exists("record.opus"):
                    os.remove("record.opus")
                os.rename(filename, "record.opus")
            
            # 使用编译好的play程序播放
            result = subprocess.run(
                ["./play"],
                capture_output=True,
                text=True,
                timeout=10
            )
            
            # 恢复文件名
            if os.path.exists("record.opus") and filename != "record.opus":
                os.rename("record.opus", filename)
            
            if result.returncode != 0:
                print(f"❌ 播放失败: {result.stderr}")
                return False
            
            print("✅ 播放完成")
            return True
            
        except subprocess.TimeoutExpired:
            print("❌ 播放超时")
            return False
        except Exception as e:
            print(f"❌ 播放异常: {e}")
            return False

    def opus_to_wav(self, opus_file, wav_file):
        """将Opus文件转换为WAV格式（用于语音识别）"""
        try:
            if not os.path.exists(opus_file):
                return False
                
            # 使用ffmpeg转换格式
            cmd = [
                "ffmpeg", "-i", opus_file,
                "-ar", "16000", "-ac", "1",
                "-f", "wav", wav_file,
                "-y"  # 覆盖输出文件
            ]
            
            result = subprocess.run(cmd, capture_output=True, timeout=10)
            return result.returncode == 0
            
        except Exception as e:
            print(f"❌ 格式转换失败: {e}")
            return False

    def recognize_audio_file(self, audio_file):
        """识别音频文件中的语音"""
        try:
            # 转换为WAV格式
            wav_file = "temp_audio.wav"
            if not self.opus_to_wav(audio_file, wav_file):
                return ""
            
            if not os.path.exists(wav_file):
                return ""
            
            # 使用vosk识别
            import wave
            wf = wave.open(wav_file, "rb")
            
            # 检查音频格式
            if wf.getnchannels() != 1 or wf.getsampwidth() != 2:
                print("❌ 不支持的音频格式")
                return ""
            
            # 创建识别器
            from vosk import KaldiRecognizer
            rec = KaldiRecognizer(self.model, wf.getframerate())
            
            # 识别音频
            results = []
            while True:
                data = wf.readframes(4000)
                if len(data) == 0:
                    break
                if rec.AcceptWaveform(data):
                    result = json.loads(rec.Result())
                    if result['text']:
                        results.append(result['text'])
            
            # 最终结果
            final_result = json.loads(rec.FinalResult())
            if final_result['text']:
                results.append(final_result['text'])
            
            wf.close()
            
            # 清理临时文件
            if os.path.exists(wav_file):
                os.remove(wav_file)
            
            recognized_text = " ".join(results).strip()
            return recognized_text
            
        except Exception as e:
            print(f"❌ 语音识别失败: {e}")
            # 清理临时文件
            if os.path.exists("temp_audio.wav"):
                os.remove("temp_audio.wav")
            return ""

    def record_voice_command(self, duration=3):
        """录制语音命令（使用C程序）"""
        temp_file = "command.opus"
        
        # 录制音频
        if not self.record_audio_file(duration, temp_file):
            return ""
        
        # 识别音频
        command = self.recognize_audio_file(temp_file)
        
        # 清理临时文件
        if os.path.exists(temp_file):
            os.remove(temp_file)
        
        return command

    def analyze_voice_command(self, text):
        """分析语音命令并生成舵机控制指令"""
        system_prompt = """你是一个机械臂控制专家。请将用户的语音命令转换为具体的舵机控制指令。

机械臂有5个舵机：
1. base (底座): 0-180度，控制左右旋转
2. shoulder (肩部): 0-180度，控制上下俯仰
3. elbow (肘部): 0-180度，控制前后伸展
4. wrist (腕部): 0-180度，控制手腕旋转
5. gripper (夹爪): 0-180度，0为闭合，180为完全打开

请根据用户指令，生成JSON格式的控制指令，包含以下字段：
- action: 动作描述
- commands: 舵机控制列表，每个命令包含 servo(舵机名称) 和 angle(目标角度)
- duration: 执行时间(秒)

示例指令：
用户说："向右转30度"
输出：{"action": "底座向右旋转", "commands": [{"servo": "base", "angle": 120}], "duration": 2}

用户说："抬起手臂"
输出：{"action": "抬起机械臂", "commands": [{"servo": "shoulder", "angle": 45}, {"servo": "elbow", "angle": 135}], "duration": 3}

用户说："抓取物体"
输出：{"action": "抓取物体", "commands": [{"servo": "gripper", "angle": 0}], "duration": 1}

请只返回JSON格式，不要其他文字。"""

        headers = {
            "Authorization": f"Bearer {self.api_key}",
            "Content-Type": "application/json"
        }
        
        data = {
            "model": self.model_name,
            "messages": [
                {"role": "system", "content": system_prompt},
                {"role": "user", "content": f"当前状态: {self.arm_state}。指令: {text}"}
            ],
            "temperature": 0.3,
            "max_tokens": 500
        }
        
        try:
            print("🔄 解析语音指令...")
            response = requests.post(
                f"{self.base_url}/chat/completions",
                headers=headers,
                json=data,
                timeout=30
            )
            
            if response.status_code == 200:
                result = response.json()
                response_text = result["choices"][0]["message"]["content"].strip()
                
                # 提取JSON部分
                try:
                    command = json.loads(response_text)
                    return command
                except:
                    import re
                    json_match = re.search(r'\{.*\}', response_text, re.DOTALL)
                    if json_match:
                        command = json.loads(json_match.group())
                        return command
                    else:
                        return {"error": "无法解析AI响应"}
            else:
                return {"error": f"API调用失败: {response.status_code}"}
                
        except Exception as e:
            return {"error": f"API调用异常: {e}"}
    
    def execute_commands(self, command_data):
        """执行舵机控制命令"""
        if "error" in command_data:
            error_msg = f"错误: {command_data['error']}"
            print(f"❌ {error_msg}")
            self.speak_and_wait(error_msg)
            return False
        
        action = command_data.get('action', '未知动作')
        duration = command_data.get('duration', 2)
        
        # 语音播报将要执行的动作
        confirm_text = f"即将执行动作：{action}"
        self.speak_and_wait(confirm_text)
        
        # 语音确认
        confirm = self.voice_confirm("请说确认执行或取消执行")
        
        if confirm == "确认":
            print(f"🎯 执行动作: {action}")
            print(f"⏱️  预计时间: {duration}秒")
            
            commands = command_data.get("commands", [])
            
            for cmd in commands:
                servo = cmd.get("servo", "")
                angle = cmd.get("angle", 90)
                
                if servo in self.arm_state:
                    print(f"   🔧 控制 {servo} 舵机: {self.arm_state[servo]}° → {angle}°")
                    self.arm_state[servo] = angle
                else:
                    print(f"   ⚠️  未知舵机: {servo}")
            
            # 执行前语音提示
            self.speak_and_wait("开始执行")
            
            # 模拟执行时间
            print(f"   ⏳ 执行中...", end="", flush=True)
            for i in range(duration):
                print(".", end="", flush=True)
                time.sleep(1)
            print(" 完成!")
            
            # 执行完成语音提示
            self.speak_and_wait("完成")
            
            print(f"📍 更新后状态: {self.arm_state}")
            return True
        else:
            cancel_msg = "取消执行"
            print(f"⏹️  {cancel_msg}")
            self.speak_and_wait(cancel_msg)
            return False
    
    def voice_confirm(self, prompt):
        """语音确认"""
        self.speak_and_wait(prompt)
        print(f"\n🎤 {prompt}...")
        
        max_attempts = 2
        for attempt in range(max_attempts):
            command = self.record_voice_command(duration=5)
            
            if not command:
                self.speak_and_wait("没听到，请重说")
                continue
                
            print(f"🗣️  识别结果: {command}")
            
            if any(word in command for word in ["确认", "执行", "是的", "好的", "开始", "确定"]):
                return "确认"
            elif any(word in command for word in ["取消", "停止", "不要", "退出", "不"]):
                return "取消"
            else:
                self.speak_and_wait("请说确认或取消")
        
        return "取消"
    
    def control_servo(self, servo_name, angle):
        """实际控制舵机的函数（需要根据具体硬件实现）"""
        print(f"[硬件控制] {servo_name} 设置为 {angle}°")
    
    def test_audio_system(self):
        """测试音频系统"""
        print("\n🎵 测试音频系统...")
        
        # 测试录音
        if not self.record_audio_file(2, "test_record.opus"):
            print("❌ 录音测试失败")
            return False
        
        # 测试播放
        if not self.play_audio_file("test_record.opus"):
            print("❌ 播放测试失败")
            return False
        
        # 测试识别
        print("🔊 请说话进行识别测试...")
        test_command = self.record_voice_command(3)
        if test_command:
            print(f"✅ 识别测试成功: {test_command}")
        else:
            print("❌ 识别测试失败")
        
        # 清理测试文件
        if os.path.exists("test_record.opus"):
            os.remove("test_record.opus")
        
        return True

    def interactive_control(self):
        """交互式控制模式"""
        self.speak_and_wait("欢迎使用机械臂语音控制系统")
        
        while True:
            print("\n" + "=" * 50)
            print("🤖 机械臂语音控制系统")
            print("=" * 50)
            print(f"📍 当前状态: {self.arm_state}")
            print("\n请选择模式:")
            print("1. 🎤 语音控制")
            print("2. 📋 预设动作") 
            print("3. 🛑 复位")
            print("4. 🔧 测试音频")
            print("5. ❌ 退出")
            
            try:
                choice = input("请输入选择 (1-5): ").strip()
                
                if choice == "1":
                    self.voice_control_mode()
                elif choice == "2":
                    self.preset_actions_mode()
                elif choice == "3":
                    self.reset_arm()
                elif choice == "4":
                    self.test_audio_system()
                elif choice == "5":
                    self.speak_and_wait("再见")
                    break
                else:
                    print("无效选择，请重新输入")
                    
            except KeyboardInterrupt:
                break

    def voice_control_mode(self):
        """语音控制模式"""
        self.speak_and_wait("语音控制模式")
        
        command = self.record_voice_command()
        
        if not command:
            self.speak_and_wait("没听到指令")
            return
        
        print(f"🗣️  指令: {command}")
        self.speak_and_wait(f"收到指令 {command}")
        
        command_data = self.analyze_voice_command(command)
        
        print(f"🔍 解析: {command_data.get('action', '未知')}")
        
        self.execute_commands(command_data)

    def preset_actions_mode(self):
        """预设动作模式"""
        self.speak_and_wait("预设动作模式")
        
        print("\n预设动作:")
        print("1. 握手")
        print("2. 抓取")
        print("3. 放置")
        print("4. 扫描")
        
        try:
            choice = input("请选择动作 (1-4): ").strip()
            
            preset_commands = {
                "1": {
                    "action": "握手",
                    "commands": [
                        {"servo": "shoulder", "angle": 60},
                        {"servo": "elbow", "angle": 120},
                        {"servo": "gripper", "angle": 90}
                    ],
                    "duration": 2
                },
                "2": {
                    "action": "抓取",
                    "commands": [
                        {"servo": "shoulder", "angle": 45},
                        {"servo": "elbow", "angle": 135},
                        {"servo": "gripper", "angle": 0}
                    ],
                    "duration": 2
                },
                "3": {
                    "action": "放置",
                    "commands": [
                        {"servo": "shoulder", "angle": 30},
                        {"servo": "elbow", "angle": 150},
                        {"servo": "gripper", "angle": 180}
                    ],
                    "duration": 2
                },
                "4": {
                    "action": "扫描",
                    "commands": [
                        {"servo": "base", "angle": 0},
                        {"servo": "base", "angle": 180},
                        {"servo": "base", "angle": 90}
                    ],
                    "duration": 4
                }
            }
            
            if choice in preset_commands:
                self.execute_commands(preset_commands[choice])
            else:
                print("无效选择")
        except KeyboardInterrupt:
            print("\n取消选择")

    def reset_arm(self):
        """复位机械臂"""
        self.speak_and_wait("复位机械臂")
        
        reset_commands = {
            "action": "复位",
            "commands": [
                {"servo": "base", "angle": 90},
                {"servo": "shoulder", "angle": 90},
                {"servo": "elbow", "angle": 90},
                {"servo": "wrist", "angle": 90},
                {"servo": "gripper", "angle": 0}
            ],
            "duration": 2
        }
        self.execute_commands(reset_commands)

def main():
    # 检查espeak
    print("🔊 检查espeak...")
    result = subprocess.run(['which', 'espeak'], 
                          capture_output=True, text=True)
    
    if result.returncode == 0:
        print("✅ espeak已安装")
        # 测试语音
        print("🔊 测试语音播报...")
        subprocess.Popen(['espeak', '-v', 'zh', '系统启动测试'],
                       stdout=subprocess.DEVNULL,
                       stderr=subprocess.DEVNULL)
        time.sleep(2)
    else:
        print("❌ espeak未安装")
    
    # 启动主程序
    controller = RoboticArmVoiceControl()
    controller.interactive_control()

if __name__ == "__main__":
    main()